//
// Created by ZhaoXiaoFei on 2022/8/5.
//

#ifndef POSE_GRAPH_POSE_GRAPH_HPP
#define POSE_GRAPH_POSE_GRAPH_HPP

#include <Eigen/Dense>
#include "key_frame.hpp"
#include <nav_msgs/Path.h>
#include "ThirdParty/DBoW/DBoW2.h"
#include "utility/CameraPoseVisualization.h"
#include <glog/logging.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/autodiff_local_parameterization.h>
#include <ceres/local_parameterization.h>
#include <thread>

class PoseGraph{
public:
    PoseGraph();
    ~PoseGraph(){
        t_optimization.join();
    }
    void registerPub(ros::NodeHandle &n);
    void updatePath();
    void addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop);
    void addKeyFrameIntoVoc(KeyFrame* cur_kf);
    int detectLoop(KeyFrame* cur_kf, int cur_kf_index);
    KeyFrame* getKeyFrame(int loopIndex);
    void optimize4DOF();
    void loadKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop){}
    void loadVocabulary(std::string voc_path);
    void updateKeyFrameLoop(int index, Eigen::Matrix<double, 8, 1 > &_loop_info);
    void publish();
    void savePoseGraph(){}
    void loadPoseGraph(){}
    std::list<KeyFrame*> keyFrameList;
    std::mutex m_keyframelist;
    std::mutex m_optimize_buf;
    std::queue<int> optimize_buf;
    std::thread t_optimization;

    nav_msgs::Path path[10];
    nav_msgs::Path base_path;

    CameraPoseVisualization* posegraph_visualization;
    // Q_w1_w2  T_w1_w2
    Eigen::Matrix3d r_drift;
    double yaw_drift;
    Eigen::Vector3d t_drift;

    //Q_w1_w1` T_w1_w1`
    Eigen::Matrix3d w_r_vio;
    Eigen::Vector3d w_t_vio;

    BriefDatabase bd;
    BriefVocabulary* voc;

    int sequence_cnt;
    std::vector<int> sequence_loop;
    int global_index;
    std::map<int, cv::Mat> image_pool;
    int earliest_loop_index;

    std::mutex m_drift;

    ros::Publisher pub_pg_path;
    ros::Publisher pub_base_path;
    ros::Publisher pub_pose_graph;
    ros::Publisher pub_path[10];

};
template <typename T>
T NormalizeAngle(const T& angle_degrees) {
    if (angle_degrees > T(180.0))
        return angle_degrees - T(360.0);
    else if (angle_degrees < T(-180.0))
        return angle_degrees + T(360.0);
    else
        return angle_degrees;
};

template <typename T>
void YPR2RotationMatrix(const T euler[3], T R[9])  {
    // R_w_b = (R(x) * R(-y) * R(z)).transport() = Rz.t * Ry.t * Rx.t;
    //其中 Rz Rx 形如： [1, 0 ,0                     [cos, 0 ,-sin
    //                  0, cos, sin                 0, 1, 0
    //                  0, -sin, cos]               sin, 0 ,cos]
    T y = euler[0] / T(180.0) * T(M_PI);
    T p = euler[1] / T(180.0) * T(M_PI);
    T r = euler[2] / T(180.0) * T(M_PI);

    R[0] = cos(y) * cos(p);
    R[1] = -sin(y) * cos(r) + cos(y) * sin(p) * sin(r);
    R[2] = sin(y) * sin(r) + cos(y) * sin(p) * cos(r);
    R[3] = sin(y) * cos(p);
    R[4] = cos(y) * cos(r) + sin(y) * sin(p) * sin(r);
    R[5] = -cos(y) * sin(r) + sin(y) * sin(p) * cos(r);
    R[6] = -sin(p);
    R[7] = cos(p) * sin(r);
    R[8] = cos(p) * cos(r);
}

template <typename T>
void RotationMatrix2RotationMatrixTransport(const T R[9], T R_inv[9])  {
    R_inv[0] = R[0];
    R_inv[1] = R[3];
    R_inv[2] = R[6];
    R_inv[3] = R[1];
    R_inv[4] = R[4];
    R_inv[5] = R[7];
    R_inv[6] = R[2];
    R_inv[7] = R[5];
    R_inv[8] = R[8];
}

template <typename T>
void RotationMatrixRotatePoint(const T R[9], const T t[3], T r_t[3])
{
    r_t[0] = R[0] * t[0] + R[1] * t[1] + R[2] * t[2];
    r_t[1] = R[3] * t[0] + R[4] * t[1] + R[5] * t[2];
    r_t[2] = R[6] * t[0] + R[7] * t[1] + R[8] * t[2];
};

class FourDofError{
public:
    FourDofError(double relativeYaw_, double pitch_i_, double roll_i_, double tx_, double ty_, double tz_)
    : relativeYaw(relativeYaw_), pitch_i(pitch_i_), roll_i(roll_i_), tx(tx_), ty(ty_), tz(tz_){

    }

    template <typename T>
    bool operator()(const T* const yaw_i , const T* const P_i, const T* const yaw_j , const T* const P_j, T* residuals) const {
        T t_w_ij[3];
        t_w_ij[0] = P_j[0] - P_i[0];
        t_w_ij[1] = P_j[1] - P_i[1];
        t_w_ij[2] = P_j[2] - P_i[2];

        T euler[3]; euler[0] = yaw_i[0]; euler[1] = T(pitch_i); euler[2] = T(roll_i);
        T R_w_i[9], R_i_w[9];
        //这里想直接用ceres提供的 发现有点小问题。
//        ceres::EulerAnglesToRotationMatrix(euler[3], R_w_i);
        YPR2RotationMatrix(euler, R_w_i);
        RotationMatrix2RotationMatrixTransport(R_w_i, R_i_w);
        T t_i_ij[3];
        RotationMatrixRotatePoint(R_i_w, t_w_ij, t_i_ij);

        residuals[0] = t_i_ij[0] - T(tx);
        residuals[1] = t_i_ij[1] - T(ty);
        residuals[2] = t_i_ij[2] - T(tz);
        residuals[3] = NormalizeAngle(yaw_j[0] - yaw_i[0] - T(relativeYaw));
        return true;
    }

    static ceres::CostFunction* Create(double relativeYaw_, double pitch_i_, double roll_i, double tx_, double ty_, double tz_){
        return (new ceres::AutoDiffCostFunction<FourDofError, 4, 1, 3, 1, 3>(
                new FourDofError(relativeYaw_, pitch_i_, roll_i, tx_, ty_, tz_)));
    }

    double relativeYaw, pitch_i, roll_i;
    //Eigen::Vector3d relativeT;
    double tx, ty, tz;
};

//第一次发现localparameter也可以自动求导
class AngleLocalParameterization{
public:
    template<typename T>
    bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
        *x_plus_delta = NormalizeAngle(*x + *delta);
        return true;
    }
    static ceres::LocalParameterization* Create(){
        return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
    }
};
//这么写也行把 可以试试
class Angle_LocalParameterization : public ceres::LocalParameterization{
public:
    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const {
        *x_plus_delta = NormalizeAngle(*x + *delta);
        return true;
    }

    virtual bool ComputeJacobian(const double* x, double* jacobian) const {
        Eigen::Map<Eigen::Matrix<double, 1, 1, Eigen::RowMajor>> jacobians(jacobian);
        jacobians.setZero();
        jacobians.setIdentity();
    }

    // Size of x.
    virtual int GlobalSize() const {
        return 1;
    }

    // Size of delta.
    virtual int LocalSize() const {
        return 1;
    }
};

#endif //POSE_GRAPH_POSE_GRAPH_HPP
